#include "control.h"
#include <math.h>


/* PID初始化函数 */
void PID_Init(pid_controller_t *pid, float kp, float ki, float kd, 
              float integral_max, float output_max, float output_min)
{
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->target = 0.0f;
    pid->error = 0.0f;
    pid->last_error = 0.0f;
    pid->integral = 0.0f;
    pid->integral_max = integral_max;
    pid->output_max = output_max;
    pid->output_min = output_min;
    pid->output = 0.0f;
}

/* PID参数设置函数 */
void PID_SetParams(pid_controller_t *pid, float kp, float ki, float kd)
{
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
}

/* PID目标值设置函数 */
void PID_SetTarget(pid_controller_t *pid, float target)
{
    pid->target = target;
}

/* PID计算函数 */
float PID_Calculate(pid_controller_t *pid, float current_value)
{
    float derivative;
    
    /* 计算误差 */
    pid->error = pid->target - current_value;
    
    /* 积分项计算 */
    pid->integral += pid->error;
    
    /* 积分限幅 */
    if (pid->integral > pid->integral_max)
        pid->integral = pid->integral_max;
    else if (pid->integral < -pid->integral_max)
        pid->integral = -pid->integral_max;
    
    /* 微分项计算 */
    derivative = pid->error - pid->last_error;
    
    /* PID输出计算 */
    pid->output = pid->Kp * pid->error + 
                  pid->Ki * pid->integral + 
                  pid->Kd * derivative;
    
    /* 输出限幅 */
    if (pid->output > pid->output_max)
        pid->output = pid->output_max;
    else if (pid->output < pid->output_min)
        pid->output = pid->output_min;
    
    /* 更新上一次误差 */
    pid->last_error = pid->error;
    
    return pid->output;
}

/* PID重置函数 */
void PID_Reset(pid_controller_t *pid)
{
    pid->error = 0.0f;
    pid->last_error = 0.0f;
    pid->integral = 0.0f;
    pid->output = 0.0f;
}

/* 双电机控制器初始化 */
void DualMotor_Init(dual_motor_controller_t *controller, 
                    encoder_handle_t encoder1, encoder_handle_t encoder2)
{
    /* 初始化电机1 */
    controller->motor1.encoder = encoder1;
    controller->motor1.mode = MOTOR_MODE_SPEED;
    controller->motor1.current_speed = 0;
    controller->motor1.current_position = 0.0f;
    controller->motor1.output_speed = 0;
    
    /* 初始化电机1的速度PID - 这里使用默认参数，实际使用时需要根据电机特性调整 */
    PID_Init(&controller->motor1.speed_pid, 
             0, 0, 0,      /* Kp, Ki, Kd */
             1000.0f,                 /* 积分限幅 */
             1000.0f, -1000.0f);      /* 输出限幅 */
    
    /* 初始化电机1的位置PID */
    PID_Init(&controller->motor1.position_pid, 
             5.0f, 0.1f, 0.5f,        /* Kp, Ki, Kd */
             500.0f,                  /* 积分限幅 */
             1000.0f, -1000.0f);      /* 输出限幅 */
    
    /* 初始化电机2 */
    controller->motor2.encoder = encoder2;
    controller->motor2.mode = MOTOR_MODE_SPEED;
    controller->motor2.current_speed = 0;
    controller->motor2.current_position = 0.0f;
    controller->motor2.output_speed = 0;
    
    /* 初始化电机2的速度PID */
    PID_Init(&controller->motor2.speed_pid, 
             10.0f, 1.0f, 0.1f,      /* Kp, Ki, Kd */
             1000.0f,                 /* 积分限幅 */
             1000.0f, -1000.0f);      /* 输出限幅 */
    
    /* 初始化电机2的位置PID */
    PID_Init(&controller->motor2.position_pid, 
             5.0f, 0.1f, 0.5f,        /* Kp, Ki, Kd */
             500.0f,                  /* 积分限幅 */
             1000.0f, -1000.0f);      /* 输出限幅 */
    
    /* 默认关闭控制 */
    controller->enable = 0;
}

/* 设置电机控制模式 */
void DualMotor_SetMode(dual_motor_controller_t *controller, motor_mode_t mode)
{
    controller->motor1.mode = mode;
    controller->motor2.mode = mode;
    
    /* 切换模式时重置PID */
    PID_Reset(&controller->motor1.speed_pid);
    PID_Reset(&controller->motor1.position_pid);
    PID_Reset(&controller->motor2.speed_pid);
    PID_Reset(&controller->motor2.position_pid);
}

/* 设置速度目标值 */
void DualMotor_SetSpeedTarget(dual_motor_controller_t *controller, 
                              int16_t speed1, int16_t speed2)
{
    PID_SetTarget(&controller->motor1.speed_pid, (float)speed1);
    PID_SetTarget(&controller->motor2.speed_pid, (float)speed2);
}

/* 设置位置目标值 */
void DualMotor_SetPositionTarget(dual_motor_controller_t *controller, 
                                 float position1, float position2)
{
    PID_SetTarget(&controller->motor1.position_pid, position1);
    PID_SetTarget(&controller->motor2.position_pid, position2);
}

/* 单个电机控制函数 */
static void Motor_Control(motor_controller_t *motor)
{
    /* 读取当前速度和位置 */
    motor->current_speed = Encoder_GetSpeed(motor->encoder);
    motor->current_position = Encoder_GetPosition(motor->encoder);
    
    /* 根据控制模式执行不同的控制 */
    if (motor->mode == MOTOR_MODE_SPEED) {
        /* 速度控制模式 */
        motor->output_speed = (int16_t)PID_Calculate(&motor->speed_pid, 
                                                     (float)motor->current_speed);
    }
    else if (motor->mode == MOTOR_MODE_POSITION) {
        /* 位置控制模式 */
        /* 位置环输出作为速度环的目标值 */
        float speed_target = PID_Calculate(&motor->position_pid, 
                                          motor->current_position);
        PID_SetTarget(&motor->speed_pid, speed_target);
        motor->output_speed = (int16_t)PID_Calculate(&motor->speed_pid, 
                                                     (float)motor->current_speed);
    }
}

/* 双电机控制主函数 */
void DualMotor_Control(dual_motor_controller_t *controller)
{
    if (!controller->enable) {
        /* 控制未使能，停止电机 */
        Moter_SetSpeed(0, 0, 0, 0);
        return;
    }
    
    /* 控制电机1 */
    Motor_Control(&controller->motor1);
    
    /* 控制电机2 */
    Motor_Control(&controller->motor2);
    
    /* 设置电机速度 */
    Moter_SetSpeed(controller->motor1.output_speed, 
                   controller->motor2.output_speed, 
                   0, 0);
}

/* 启动/停止电机控制 */
void DualMotor_Enable(dual_motor_controller_t *controller, uint8_t enable)
{
    controller->enable = enable;
    
    if (!enable) {
        /* 停止时重置PID */
        PID_Reset(&controller->motor1.speed_pid);
        PID_Reset(&controller->motor1.position_pid);
        PID_Reset(&controller->motor2.speed_pid);
        PID_Reset(&controller->motor2.position_pid);
    }
}

/* 设置速度PID参数 */
void DualMotor_SetSpeedPID(dual_motor_controller_t *controller, uint8_t motor_id,
                          float kp, float ki, float kd)
{
    if (motor_id == 1) {
        PID_SetParams(&controller->motor1.speed_pid, kp, ki, kd);
    }
    else if (motor_id == 2) {
        PID_SetParams(&controller->motor2.speed_pid, kp, ki, kd);
    }
}

/* 设置位置PID参数 */
void DualMotor_SetPositionPID(dual_motor_controller_t *controller, uint8_t motor_id,
                             float kp, float ki, float kd)
{
    if (motor_id == 1) {
        PID_SetParams(&controller->motor1.position_pid, kp, ki, kd);
    }
    else if (motor_id == 2) {
        PID_SetParams(&controller->motor2.position_pid, kp, ki, kd);
    }
}
